/*
 * FormationSquare.java
 *
 * Created on 18 de noviembre de 2007, 07:04 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package nx.formations;


import nx.utils.Vector2D;
/**
 *
 * @author alejo
 */
public class FormationArrow extends Formation {
    
    
    /** Creates a new instance of FormationSquare */
    public FormationArrow(int numRobots) {
        super(numRobots);
        
        setWidth(2); //width of form
        this.setName("ARROW");
    }
    
    public Vector2D getRobotPosition(int robotId){
        if(robotId>=0 && robotId<=this.getNumRobots()){
            
            //get angle of location in Formation
            float theta = (float)getRobotLocationDir(robotId);
            
            return getVectorInForm(theta,robotId);
            
        }
            
        return null;
    
    }
    

    
    public Vector2D getVectorInForm(float theta, int robotId){
        
        float x = 0;
        float y = 0;
        
        /*
        switch(robotId){
            
            case 0:
                x = (float)(Math.sqrt(2)/4.0f);
                y = (float)(Math.sqrt(2)/4.0f);
            break;
            case 1:
                x = -(float)(Math.sqrt(2)/2.0f);
                y = (float)(Math.sqrt(2)/2.0f);
            break;
            case 2:
                x = -(float)(Math.sqrt(2)/2.0f);
                y = -(float)(Math.sqrt(2)/2.0f);
            break;
            case 3:
                x = (float)(Math.sqrt(2)/4.0f);
                y = -(float)(Math.sqrt(2)/4.0f);
            break;
            
        }
        */
        if(theta >= 0 && theta <= Math.PI - Math.PI / 4){
            x = (float)(1 / (2*Math.tan(theta)+1));
            y = (float)(-1 / (4*Math.tan(theta)+2)+0.5);
        
        }
        
        else if(theta > (Math.PI - Math.PI / 4) && theta <= (Math.PI + Math.PI / 4) ){
        
            x = -1;
            y = (float)-Math.tan(theta);
        }
        else{
        
            x = (float)(1 / (-2*Math.tan(theta)+1));
            y = (float)(1 / (-4*Math.tan(theta)+2)-0.5);
        }
        
         
        if(robotId>=this.getNumRobots())
            return null;
        else
            return new Vector2D(x,y,Vector2D.MODE_XY);
        
    
    }
    
    public void calculateScaleFactor()
    {
        scaleFactor = (float)Math.sqrt((double)(width*width/2));
    }
    

   
    
}
